Multiagent Simulations

RoboticSystems.jl makes it very easy to define multiagent simulations. This is achieved through ComponentArrays and DiffEq working well together.

We will simulate a quadrotor flying in a circle, and a second quadrotor trying to circle around it.

Define the quadrotors

using ComponentArrays, Plots, LinearAlgebra, Parameters
using RoboticSystems
RS = RoboticSystems

gr()

# define the quadrotor parameters
quad1_p = ComponentArray(
  RS.quadrotor_parameters;
  mass=1.5) |> RS.initialize_quad_params

quad2_p = ComponentArray(
  RS.quadrotor_parameters;
  mass=0.8) |> RS.initialize_quad_params


quad1_ic = ComponentArray(
    x = zeros(3),
    v = zeros(3),
    R = 1.0(I(3)) |> collect,
    Ω = zeros(3),
    ω = zeros(4)
);

quad2_ic = ComponentArray(
    x = [1,0,0.], # this one starts at a different location
    v = zeros(3),
    R = 1.0(I(3)) |> collect,
    Ω = zeros(3),
    ω = zeros(4)
);

##Plot the quads
plot()
RS.plot_quad!(quad1_ic, quad1_p)
RS.plot_quad!(quad2_ic, quad2_p)
RS.plot_iso3d!()

Define the controller for the first quad

cont1_p = ComponentArray(
    kx = 1.0,
    kv = 2.0,
    kR = 0.35,
    kΩ = 0.15,
    m = quad1_p.mass,
    J = quad1_p.J,
    invG = quad1_p.invG,
    g = 9.81
)

function controller1(state, cont_p, t)

    ## Circular Trajectory
    w = 2*π/10
    xd = [sin(w*t), sin(w*t+π/2), 1]
    vd = [w*sin(w*t + π/2), w*sin(w*t + π/2 + π/2), 0]
    ad = [w^2*sin(w*t + 2*π/2), w^2*sin(w*t + π/2 + 2*π/2), 0]
    jd = [w^3*sin(w*t + 3*π/2), w^3*sin(w*t + π/2 + 3*π/2), 0]
    sd = [w^4*sin(w*t + 4*π/2), w^4*sin(w*t + π/2 + 4*π/2), 0]

    ## define the desired yaw, yaw rate, and yaw acceleration at current time
    ψ = 0.0
    ψd = 0.0
    ψdd = 0.0

    ## convert into target quadrotor state,  using differential flatness
    target = RS.flat_state_to_quad_state(xd, vd, ad, jd, sd, ψ, ψd, ψdd)

    ## determine required thrust and moments, using geometric controller
    fM = RS.geometric_controller(state, target..., cont_p)

    ## determine rotation speed of each motor
    control = RS.fM_to_ω(fM, cont_p)

end

cont2_p = ComponentArray(
    kx = 1.0,
    kv = 2.0,
    kR = 0.35,
    kΩ = 0.15,
    m = quad2_p.mass,
    J = quad2_p.J,
    invG = quad2_p.invG,
    g = 9.81
)

function controller2(state, cont_p, quad1_pos, t)

    ## Maintain a position relative to quad 1
    xd = quad1_pos + [0; 0 ; 1.]
    vd = zeros(3)
    ad = zeros(3)
    jd = zeros(3)
    sd = zeros(3)

    ## define the desired yaw, yaw rate, and yaw acceleration at current time
    ψ = 0.0
    ψd = 0.0
    ψdd = 0.0

    ## convert into target quadrotor state,  using differential flatness
    target = RS.flat_state_to_quad_state(xd, vd, ad, jd, sd, ψ, ψd, ψdd)

    ## determine required thrust and moments, using geometric controller
    fM = RS.geometric_controller(state, target..., cont_p)

    ## determine rotation speed of each motor
    control = RS.fM_to_ω(fM, cont_p)

end


function closed_loop!(D, state, params, t)

    @unpack quad1, quad2 = state
    @unpack quad1_p, quad2_p, cont1_p, cont2_p = params

    u1 = controller1(quad1, cont1_p, t)
    u2 = controller2(quad2, cont2_p, quad1.x, t)

    # update the dynamics
    RS.quadrotor!(D.quad1, quad1, quad1_p, u1, t)
    RS.quadrotor!(D.quad2, quad2, quad2_p, u2, t)

    return
end

closed_loop! (generic function with 1 method)

Perform the Simulation

using DifferentialEquations

tspan = (0, 25.0)

# combine the states
full_state_ic = ComponentArray(
  quad1 = quad1_ic,
  quad2 = quad2_ic
)

# combine all the parameters
params = ComponentArray(
  quad1_p = quad1_p,
  quad2_p = quad2_p,
  cont1_p = cont1_p,
  cont2_p = cont2_p,
)


## try the closed loop function
D_ = similar(full_state_ic)
closed_loop!(D_, full_state_ic, params, 0.0)

# define the ODE problem
prob = ODEProblem(closed_loop!, full_state_ic, tspan, params)

## Solve the problem
sol = solve(prob, Tsit5())


## Plot the trajectory
plot()
RS.plot_quad_traj!(sol, quad1_p; tspan=tspan, prefix="quad1")
RS.plot_quad_traj!(sol, quad2_p; tspan=tspan, prefix="quad2")
RS.plot_iso3d!()
RS.plot_project3d!()

anim = @animate for tm = range(sol.t[1], sol.t[end], 200)

    tspan=(sol.t[1], tm)

    plot()

    RS.plot_quad_traj!(sol, quad1_p; tspan=tspan, prefix="quad1")
    RS.plot_quad_traj!(sol, quad2_p; tspan=tspan, prefix="quad2")

    plot!(xlims=(-1.5, 1.5), ylims=(-1.5, 1.5), zlims=(0, 2.5))

    # rotate camera
    plot!(camera = (360 * ((tm - sol.t[1]) / (sol.t[end] - sol.t[1])), 30))
    RS.plot_iso3d!()
    RS.plot_project3d!()

end;

gif(anim)